#include "ImmUkf.hpp"
#include "global.h"
int debug_pred_id = 0;

#include <ros/ros.h>

inline void cropAngle(double& a)
{
	while (a> M_PI) a -= 2.*M_PI;
	while (a<-M_PI) a += 2.*M_PI;
}


VectorXd getCpFromBbox(PointCloud<PointXYZ> bBox){
    PointXYZ p1 = bBox[0];
    PointXYZ p2 = bBox[1];
    PointXYZ p3 = bBox[2];
    PointXYZ p4 = bBox[3];

    double S1 = ((p4.x -p2.x)*(p1.y - p2.y) - (p4.y - p2.y)*(p1.x - p2.x))/2;
    double S2 = ((p4.x -p2.x)*(p2.y - p3.y) - (p4.y - p2.y)*(p2.x - p3.x))/2;
    double cx = p1.x + (p3.x-p1.x)*S1/(S1+S2);
    double cy = p1.y + (p3.y-p1.y)*S1/(S1+S2);

    VectorXd cp(2);
    cp << cx, cy;
    return cp;
}


void ImmUkf::initEgoTracking()
{
	_egoUkf = UKF(_ukf_params);
}


ImmUkf::ImmUkf()
{
	_targets.resize(_targetCapacity);
	for(int i=0; i<_targetCapacity; i++)    _targets[i] = nullptr;
}

ImmUkf::~ImmUkf()
{
	for(int i=0; i<_targetCapacity; i++){
		if(_targets[i] != nullptr)	delete _targets[i];
	}
}

void egoPolyfit(queue<pair<double, Vector2d>> qtxy, Vector2d ego, vector<vector<double>>& ego_traj)
{
	int n = qtxy.size();

	if(n < 6)	return;

	double first_time = qtxy.front().first;
	Vector2d first_xy = qtxy.front().second;
	qtxy.pop();

	MatrixXd A(n-1, 4);
	VectorXd xb(n-1);
	VectorXd yb(n-1);
	VectorXd xcof, ycof;
	A.col(0).fill(1.0);
	for(int i=0; i<n-1; i++){
		auto txy = qtxy.front();
		qtxy.pop();
		double tmp = 1.0;
		double dt = txy.first - first_time;
		for(int j=1; j<4; j++)	A(i, j) = A(i, j-1) * dt;
		xb(i) = txy.second(0) - first_xy(0);
		yb(i) = txy.second(1) - first_xy(1);
	}
	//cout << A << endl;
	//cout << "----------------" << endl;
	// solve
	xcof = A.householderQr().solve(xb);
	ycof = A.householderQr().solve(yb);

	// cout << "xcof: ";
	// for(int i=0; i<4; i++)  cout << xcof(i) << " ";
	// cout << endl;
	// cout << "ycof: ";
	// for(int i=0; i<4; i++)  cout << ycof(i) << " ";
	// cout << endl;

	// check for nan
	if(isnanf(xcof.sum()) || isnanf(ycof.sum()))	return;

	double predtime = 5;
	double preddt = 0.2;

	int pred_num = int(predtime / preddt);

	vector<double> state = {ego(0), ego(1)};
	ego_traj.emplace_back(state);

	for(int i = 0; i<pred_num; i++)
	{
		VectorXd pt(4);
		pt(0) = 1;
		for (int j = 1; j < 4; j++)		pt(j) = pt(j-1) * preddt * (i+1);
		double pred_x = xcof.dot(pt);
		double pred_y = ycof.dot(pt);

		state[0] = pred_x + ego(0);
		state[1] = pred_y + ego(1);
		ego_traj.emplace_back(state);

	}
	
}

void ImmUkf::setInput(double timestamp, const Vector3d& ego, const vector<PointCloud<PointXYZ>>& input)
{
	_trackCp.clear();
	_trackBoxes.clear();

	_trackCp.reserve(input.size());
	_trackBoxes.reserve(input.size());

	for(const auto& e : input)
	{
		VectorXd cp = getCpFromBbox(e);
		VectorXd cp2ego = cp - ego.head<2>();
		if(cp2ego.norm() < _inputMeasDistTh)
		{
			_trackCp.emplace_back(cp);
			_trackBoxes.emplace_back(e);
		}
	}

	 if(!_egoUkf.is_initialized_)
		 _egoUkf.initializeForEgo(ego, timestamp);
	 else{
		 VectorXd z = ego.head<2>();
		 _egoUkf.ProcessIMMUKF(timestamp, false);
		 for(int i=0; i<3; i++) 	_egoUkf.singleUpdate(z, i);
		
		 VectorXd diffCV   = z - _egoUkf.zPredCVl_;
		 VectorXd diffCTRV = z - _egoUkf.zPredCTRVl_;
		 VectorXd diffRM   = z - _egoUkf.zPredRMl_;
		 ROS_DEBUG_NAMED("ego", "[ego] diff: (%f, %f, %f)", diffCV.norm(), diffCTRV.norm(), diffRM.norm());
		 // double eCV   = diffCV.transpose()*  _egoUkf.lS_cv_.inverse()  *diffCV;
		 // double eCTRV = diffCTRV.transpose()*  _egoUkf.lS_ctrv_.inverse()*diffCTRV;
		 // double eRM   = diffRM.transpose()*  _egoUkf.lS_rm_.inverse()  *diffRM;
		 double eCV   = exp(  -0.5*diffCV.transpose()*  _egoUkf.lS_cv_.inverse()  *diffCV);
		 double eCTRV = exp(-0.5*diffCTRV.transpose()*  _egoUkf.lS_ctrv_.inverse()*diffCTRV);
		 double eRM   = exp(  -0.5*diffRM.transpose()*  _egoUkf.lS_rm_.inverse()  *diffRM);
		 // double sum = eCV + eCTRV + eRM;
		 // eCV = 1 - eCV/sum;
		 // eCTRV = 1 - eCTRV/sum;
		 // eRM = 1 - eRM/sum;
		 ROS_DEBUG_NAMED("ego", "[ego] lambdaVec: (%f, %f, %f)", eCV, eCTRV, eRM);
		 vector<double> lambdaVec = {eCV, eCTRV, eRM};
		 _egoUkf.PostProcessIMMUKF(lambdaVec);
		 _egoUkf.predictTraj(_predTime, ego);
	 }

	// try ployfit
	//ego_xy_history.push(make_pair(timestamp, ego.head<2>()));
	//if(ego_xy_history.size() > 8)	ego_xy_history.pop();
	//ego_traj.clear();
	//egoPolyfit(ego_xy_history, ego.head<2>(), ego_traj);

}


void ImmUkf::findBestZandS(int id, VectorXd& bestZ, MatrixXd& maxDetS){
    const UKF& tg = *(_targets[id]);
	
	VectorXd z[3] = {tg.zPredCVl_, tg.zPredCTRVl_, tg.zPredRMl_};
	MatrixXd S[3] = {tg.lS_cv_, tg.lS_ctrv_, tg.lS_rm_};

	double cv_mode   = tg.modeProbCV_;
    double ctrv_mode = tg.modeProbCTRV_;
    double rm_mode   = tg.modeProbRM_;

	double det[3] = {cv_mode, ctrv_mode, rm_mode};
	int maxId = max_element(det, det+3) - det;

	bestZ = z[maxId];
	maxDetS = S[maxId];
}

void ImmUkf::findBestZandMaxS(int id, VectorXd& bestZ, MatrixXd& maxDetS){
    const UKF& tg = *(_targets[id]);
	
	VectorXd z[3] = {tg.zPredCVl_, tg.zPredCTRVl_, tg.zPredRMl_};
	MatrixXd S[3] = {tg.lS_cv_, tg.lS_ctrv_, tg.lS_rm_};

	double cv_det   = tg.lS_cv_.determinant();
    double ctrv_det = tg.lS_ctrv_.determinant();
    double rm_det   = tg.lS_rm_.determinant();

	double det[3] = {cv_det, ctrv_det, rm_det};
	int maxId = max_element(det, det+3) - det;
	int minId = min_element(det, det+3) - det;

	bestZ = z[minId];
	maxDetS = S[minId];

}

void ImmUkf::findMaxZandS(int id, VectorXd& maxDetZ, MatrixXd& maxDetS){
    const UKF& target = *(_targets[id]);
	
	double cv_det   = target.lS_cv_.determinant();
    double ctrv_det = target.lS_ctrv_.determinant();
    double rm_det   = target.lS_rm_.determinant();

    if(cv_det > ctrv_det){
        if(cv_det > rm_det)  {
            maxDetZ = target.zPredCVl_;
            maxDetS = target.lS_cv_;
        }
        else                 {
            maxDetZ = target.zPredRMl_;
            maxDetS = target.lS_rm_;
        }
    }
    else{
        if(ctrv_det > rm_det) {
            maxDetZ = target.zPredCTRVl_;
            maxDetS = target.lS_ctrv_;
        }
        else                  {
            maxDetZ = target.zPredRMl_;
            maxDetS = target.lS_rm_;
        }
    }
}


void ImmUkf::measurementValidation(int id, VectorXd maxDetZ, MatrixXd maxDetS, vector<VectorXd>& measVec, vector<bool>& measTaken, vector<int>& measIds)
{
    for(int i = 0; i < _trackCp.size(); i++){
		VectorXd mea = _trackCp[i];
        VectorXd diff = mea - maxDetZ;
        double nis = diff.transpose()*maxDetS.inverse()*diff;
		ROS_DEBUG_NAMED("run", "[run] target %d MV diff(%f, %f), nis(%f)", id, diff(0), diff(1), nis);
        if(nis < _gammaG){ // x^2 99% range
			measVec.push_back(mea);
			measIds.emplace_back(i);
			measTaken[i] = true;
        }
		
    }

}

void ImmUkf::getNearestEuclidBBox(const VectorXd& cp, const vector<VectorXd>& measVec, int& minInd){
    
	int minDist = _nearestEuclidThresh;
    for (int i = 0; i < measVec.size(); i++){
        
		VectorXd x = cp - measVec[i];
		double dist = x.norm();

		if(dist < minDist){
            minDist = dist;
            minInd = i;
        }
    }
}


// for STABLE: associate the nearest point
void ImmUkf::associateBB(int id, VectorXd maxDetZ, vector<VectorXd>& measVec, vector<int>& measIds)
{
	int minInd = -1;
    if(_targets[id]->state_ == UKF::trackingState_.STABLE){
        getNearestEuclidBBox(maxDetZ, measVec, minInd);
		if(minInd >= 0){
			VectorXd tmp = measVec[minInd];
			measVec.clear();
			measVec.emplace_back(tmp);
			int tmpId = measIds[minInd];
			measIds.clear();
			measIds.emplace_back(tmpId);
			// debug
			_targets[id]->life_time_++;
		}else{
			_targets[id]->state_++;
			_targets[id]->life_time_--;
		}
	}
}


inline double getAreaXYZ(const Vector3d& v)
{
	return v(0)*v(1)*v(2);
}

void ImmUkf::updateBB(int id, const vector<int>& measIds){
	
	// only update the target which are stable
	
	UKF& tg = *(_targets[id]);
	if(tg.state_ == UKF::trackingState_.STABLE)
	{
		int minInd = measIds[0];
		const PointCloud<PointXYZ>& ass_box = _trackBoxes[minInd];
		double yaw = tg.x_merge_yaw_;
		Vector2d v;
		v << cos(yaw), sin(yaw);

		Vector3d xyz = tg.getXYZLength(ass_box, v, 1);
		for(int i=0; i<3; i++)
			if((xyz(i) <= 0) || (xyz(i) > UKF::box_limit_xyz_[i])) 	return;

		//double cur_area = getAreaXYZ(tg.Bbox_XYZ_);
		//double new_area = getAreaXYZ(xyz);
		
		// update box according to x y change
		double& cx = tg.Bbox_XYZ_(0);	
		double& cy = tg.Bbox_XYZ_(1);
		assert(cy > 0);
		
		double rate = cx / cy;

		if(abs(cx - xyz(0)) > rate * _yUpdateBBTh){
			tg.updateBBLastTime[0]++;
			if(tg.updateBBLastTime[0] > _updateBBNum)
			{
				cx = xyz(0);
				tg.updateBBLastTime[0] = 0;
			}
		}else{
			tg.updateBBLastTime[0] = 0;
		}

		if(abs(cy - xyz(1)) > _yUpdateBBTh){
			tg.updateBBLastTime[1] ++;
			if(tg.updateBBLastTime[1] > _updateBBNum)
			{
				cy = xyz(1);
				tg.updateBBLastTime[1] = 0;
			}
		}else
		{
			tg.updateBBLastTime[1] = 0;
		}
		//double tmp = new_area / cur_area;
		//if(tmp > 1 && tmp < _large_area_th){
			//ROS_DEBUG_NAMED("updateBB", "target %d box mode 1 larger (%d/%d) (%f, %f, %f) -> (%f, %f, %f)", id, minInd, _trackBoxes.size(), tg.Bbox_XYZ_(0), tg.Bbox_XYZ_(1), tg.Bbox_XYZ_(2), xyz(0), xyz(1), xyz(2));
			//tg.Bbox_XYZ_ = xyz;
			//_large_box_cnt = 0;
		//}else if(tmp >= _large_area_th){
			//_large_box_cnt++;
			//if(_large_box_cnt > _large_box_num){
				//ROS_DEBUG_NAMED("updateBB", "target %d box mode 2 larger (%f, %f, %f) -> (%f, %f, %f)", id, tg.Bbox_XYZ_(0), tg.Bbox_XYZ_(1), tg.Bbox_XYZ_(2), xyz(0), xyz(1), xyz(2));
				////ROS_DEBUG_NAMED("updateBB", "target %d box larger!!", id);
				//tg.Bbox_XYZ_ = xyz;
				//_large_box_cnt = 0;
			//}
		//}else{
			//_large_box_cnt = 0;
		//}
	}

}


void ImmUkf::updateTrackingState(int id, int numOfMeas,	const vector<VectorXd>& measVec)
{

	int& st = _targets[id]->state_;
	const UKF& tg = *(_targets[id]);
	if(numOfMeas > 0){
		//ROS_DEBUG_NAMED("run", "target %d get meas(%d/%d) to updateState(%d)", id, numOfMeas, _trackCp.size(), st);
		//if(numOfMeas == 1){
			//ROS_DEBUG_NAMED("mul", "target %d meas(%f, %f), pred cv(%f, %f), ctrv(%f, %f), rm(%f, %f)", id, measVec[0](0), measVec[0](1), tg.zPredCVl_(0), tg.zPredCVl_(1), tg.zPredCTRVl_(0), tg.zPredCTRVl_(1), tg.zPredRMl_(0), tg.zPredRMl_(1));
		//}
		if(st < UKF::trackingState_.STABLE) 	st++;
		else if(st > UKF::trackingState_.STABLE)
		{
			st = UKF::trackingState_.STABLE;
		}
	}else{
		ROS_DEBUG_NAMED("run", "[run] target %d get none(%d) to updateState(%d)", id,  _trackCp.size(), st);
		if(st < UKF::trackingState_.STABLE) 	st++;
		st++;
	}

	if(st >= UKF::trackingState_.DRIFT) 	st = UKF::trackingState_.LOST;
}


double getIntersectCoef(double vec1x, double vec1y, double vec2x,double vec2y,
                        double px,double py, double cpx,double cpy){
    double intersectCoef = (((vec1x-vec2x)*(py - vec1y) + (vec1y - vec2y)*(vec1x - px)) *
        ((vec1x - vec2x)*(cpy - vec1y) + (vec1y - vec2y)*(vec1x - cpx)));
    return intersectCoef;

}


// void ImmUkf::mergeOverSegmentation(){
    
// 	for(int i = 0; i < _targets.size(); i++){
//         if(_targets[i] != nullptr && _targets[i]->state_ == UKF::trackingState_.STABLE){
//             double vec1x = _targets[i]->BBox_[0].x;
//             double vec1y = _targets[i]->BBox_[0].y;
//             double vec2x = _targets[i]->BBox_[1].x;
//             double vec2y = _targets[i]->BBox_[1].y;
//             double vec3x = _targets[i]->BBox_[2].x;
//             double vec3y = _targets[i]->BBox_[2].y;
//             double vec4x = _targets[i]->BBox_[3].x;
//             double vec4y = _targets[i]->BBox_[3].y;
//             double cp1x  = (vec1x+vec2x+vec3x)/3;
//             double cp1y  = (vec1y+vec2y+vec3y)/3;
//             double cp2x  = (vec1x+vec4x+vec3x)/3;
//             double cp2y  = (vec1y+vec4y+vec3y)/3;
//             for (int j = 0; j < _targets.size(); j++){
//                 if(_targets[j] == nullptr || (_targets[i]->state_ != UKF::trackingState_.STABLE) || i == j) continue;
//                 double px = _targets[j].x_merge_(0);
//                 double py = _targets[j].x_merge_(1);
//                 double cross1 = getIntersectCoef(vec1x, vec1y, vec2x, vec2y, px, py, cp1x, cp1y);
//                 double cross2 = getIntersectCoef(vec1x, vec1y, vec3x, vec3y, px, py, cp1x, cp1y);
//                 double cross3 = getIntersectCoef(vec3x, vec3y, vec2x, vec2y, px, py, cp1x, cp1y);
//                 double cross4 = getIntersectCoef(vec1x, vec1y, vec4x, vec4y, px, py, cp2x, cp2y);
//                 double cross5 = getIntersectCoef(vec1x, vec1y, vec3x, vec3y, px, py, cp2x, cp2y);
//                 double cross6 = getIntersectCoef(vec3x, vec3y, vec4x, vec4y, px, py, cp2x, cp2y);
//                 if((cross1 > 0 && cross2>0&&cross3>0)||(cross4>0 && cross5 > 0 && cross6>0)){
//                     // cout << "merrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrge"<<endl;
//                     _targets[i]->state_ = UKF::trackingState_.STABLE;
//                     _targets[j]->state_ = UKF::trackingState_.LOST;
//                 }
//             }
//         }
//     }
// }


void ImmUkf::filterPDA(int id, vector<VectorXd> measVec, vector<double>& lambdaVec){
    // calculating association probability

	UKF& target = *(_targets[id]);

	//ROS_DEBUG_NAMED("filter", "target %d filter meas(%d), (%f, %f)", id, measVec.size(), measVec[0](0), measVec[0](1));

    double numMeas = measVec.size();
	double b = 2*numMeas*(1-_pD*_pG)/(_gammaG*_pD);
	//double b = 0;
	double eCVSum   = 1e-10;
	double eCTRVSum = 1e-10;
    double eRMSum   = 1e-10;

    vector<double> eCvVec;
    vector<double> eCtrvVec;
    vector<double> eRmVec;

    vector<VectorXd> diffCVVec;
    vector<VectorXd> diffCTRVVec;
    vector<VectorXd> diffRMVec;

    for(int i = 0; i < numMeas; i++){
        VectorXd diffCV   = measVec[i] - target.zPredCVl_;
        VectorXd diffCTRV = measVec[i] - target.zPredCTRVl_;
        VectorXd diffRM   = measVec[i] - target.zPredRMl_;

        diffCVVec.push_back(diffCV);
        diffCTRVVec.push_back(diffCTRV);
        diffRMVec.push_back(diffRM);

        double eCV   = exp(  -0.5*diffCV.transpose()*  target.lS_cv_.inverse()  *diffCV);
        double eCTRV = exp(-0.5*diffCTRV.transpose()*  target.lS_ctrv_.inverse()*diffCTRV);
        double eRM   = exp(  -0.5*diffRM.transpose()*  target.lS_rm_.inverse()  *diffRM);
	
        eCvVec.push_back(eCV);
        eCtrvVec.push_back(eCTRV);
        eRmVec.push_back(eRM);

        eCVSum   += eCV;
        eCTRVSum += eCTRV;
        eRMSum   += eRM;
    }
    double betaCVZero   = b/(b+eCVSum);
    double betaCTRVZero = b/(b+eCTRVSum);
    double betaRMZero   = b/(b+eRMSum);

    vector<double> betaCV;
    vector<double> betaCTRV;
    vector<double> betaRM;
    // cout << "beta cv" << endl;

    for(int i = 0; i < numMeas; i++){
        double tempCV   = eCvVec[i]/(b+eCVSum);
        double tempCTRV = eCtrvVec[i]/(b+eCTRVSum);
        double tempRM   = eRmVec[i]/(b+eRMSum);
        // cout << tempCV << endl;
        betaCV.push_back(tempCV);
        betaCTRV.push_back(tempCTRV);
        betaRM.push_back(tempRM);
    }

    VectorXd sigmaXcv;
    VectorXd sigmaXctrv;
    VectorXd sigmaXrm;
    sigmaXcv.setZero(2);
    sigmaXctrv.setZero(2);
    sigmaXrm.setZero(2);

    for(int i = 0; i < numMeas; i++){
        sigmaXcv   += betaCV[i]*diffCVVec[i];
        sigmaXctrv += betaCTRV[i]*diffCTRVVec[i];
        sigmaXrm   += betaRM[i]*diffRMVec[i];
    }

	//if(numMeas >= 1){
		//ROS_DEBUG_NAMED("upda", "traj(%d) betaRM(%f) diffRMVec(%f, %f)", debug_pred_id, betaRM[0], diffRMVec[0](0), diffRMVec[0](1));
	//}
   
	MatrixXd sigmaPcv;
    MatrixXd sigmaPctrv;
    MatrixXd sigmaPrm;
    sigmaPcv.setZero(2,2);
    sigmaPctrv.setZero(2,2);
    sigmaPrm.setZero(2,2);
    for(int i = 0; i < numMeas; i++){
        sigmaPcv   += (betaCV[i]  *diffCVVec[i]  *diffCVVec[i].transpose()     - sigmaXcv*sigmaXcv.transpose());
        sigmaPctrv += (betaCTRV[i]*diffCTRVVec[i]*diffCTRVVec[i].transpose()   - sigmaXctrv*sigmaXctrv.transpose());
        sigmaPrm   += (betaRM[i]  *diffRMVec[i]  *diffRMVec[i].transpose()     - sigmaXrm*sigmaXrm.transpose());
    }
    
    // update x and P
    target.x_cv_   = target.x_cv_   + target.K_cv_*sigmaXcv;
    target.x_ctrv_ = target.x_ctrv_ + target.K_ctrv_*sigmaXctrv;
    target.x_rm_   = target.x_rm_   + target.K_rm_*sigmaXrm;

	//if(numMeas >= 1)
		//ROS_DEBUG_NAMED("upda", "(upda, %d, %f, %f)", 3, measVec[0](0), measVec[0](1));
	
	//ROS_DEBUG_NAMED("upda", "(upda, %d, %f, %f)", 0, target.x_cv_(0), target.x_cv_(1));
	//ROS_DEBUG_NAMED("upda", "(upda, %d, %f, %f)", 1, target.x_ctrv_(0), target.x_ctrv_(1));
	//ROS_DEBUG_NAMED("upda", "(upda, %d, %f, %f)", 2, target.x_rm_(0), target.x_rm_(1));
	
	cropAngle(target.x_cv_(3));
	cropAngle(target.x_ctrv_(3));
	cropAngle(target.x_rm_(3));
   
    if(numMeas != 0){
    	target.P_cv_   = betaCVZero*target.P_cv_ +
                  (1-betaCVZero)*(target.P_cv_ - target.K_cv_*target.lS_cv_*target.K_cv_.transpose()) +
                  target.K_cv_*sigmaPcv*target.K_cv_.transpose();
	    target.P_ctrv_ = betaCTRVZero*target.P_ctrv_ +
	                  (1-betaCTRVZero)*(target.P_ctrv_ - target.K_ctrv_*target.lS_ctrv_*target.K_ctrv_.transpose()) +
	                  target.K_ctrv_*sigmaPctrv*target.K_ctrv_.transpose();
	    target.P_rm_   = betaRMZero*target.P_rm_ +
	                  (1-betaRMZero)*(target.P_rm_ - target.K_rm_*target.lS_rm_*target.K_rm_.transpose()) +
	                  target.K_rm_*sigmaPrm*target.K_rm_.transpose();
    }
    else{
    	target.P_cv_   = target.P_cv_   - target.K_cv_  *target.lS_cv_  *target.K_cv_.transpose();
	    target.P_ctrv_ = target.P_ctrv_ - target.K_ctrv_*target.lS_ctrv_*target.K_ctrv_.transpose();
	    target.P_rm_   = target.P_rm_   - target.K_rm_  *target.lS_rm_  *target.K_rm_.transpose();
    }
    // cout << "after update p cv: "<<endl << target.P_cv_ << endl;
     
    VectorXd maxDetZ;
    MatrixXd maxDetS;
    // cout << endl<<"filterPDA" << endl;
    findMaxZandS(id, maxDetZ, maxDetS);
    double Vk =  M_PI * sqrt(abs(_gammaG * maxDetS.determinant()));

    double lambdaCV, lambdaCTRV, lambdaRM;
    if(numMeas != 0){
		double cv_det = abs(target.lS_cv_.determinant());
		double ctrv_det = abs(target.lS_ctrv_.determinant());
		double rm_det = abs(target.lS_rm_.determinant());
		
    	lambdaCV   = (1 - _pG*_pD)/pow(Vk, numMeas) +
	                        _pD*pow(Vk, 1-numMeas)*eCVSum/(numMeas*sqrt(2*M_PI*cv_det));
	    lambdaCTRV = (1 - _pG*_pD)/pow(Vk, numMeas) +
	                        _pD*pow(Vk, 1-numMeas)*eCTRVSum/(numMeas*sqrt(2*M_PI*ctrv_det));
	    lambdaRM   = (1 - _pG*_pD)/pow(Vk, numMeas) +
	                        _pD*pow(Vk, 1-numMeas)*eRMSum/(numMeas*sqrt(2*M_PI*rm_det));
    }
    else{
    	lambdaCV   = (1 - _pG*_pD)/pow(Vk, numMeas);
	    lambdaCTRV = (1 - _pG*_pD)/pow(Vk, numMeas);
	    lambdaRM   = (1 - _pG*_pD)/pow(Vk, numMeas);
    }
    // cout <<endl<< "lambda: "<<endl<<lambdaCV << " "<< lambdaCTRV<<" "<< lambdaRM << endl;
    lambdaVec.push_back(lambdaCV);
    lambdaVec.push_back(lambdaCTRV);
    lambdaVec.push_back(lambdaRM);

	//ROS_DEBUG_NAMED("traj", "target %d lambda(%f, %f, %f)", id, lambdaCV, lambdaCTRV, lambdaRM);
}

void ImmUkf::updateTargets(const vector<bool>& measTaken, double timeStamp)
{
	vector<int> notTakenIds;

	for(int i=0; i<measTaken.size(); i++)
	{
		if(!measTaken[i]){
			bool eval = true;
			// evaluate the measurement, maybe it's too close to target
			const VectorXd& mea = _trackCp[i];
			for(const auto& e : _targets){
				if(e != nullptr){
					VectorXd tmp = mea - e->x_merge_.block<2, 1>(0, 0);
					if(tmp.norm() < _newTargetDistTh) 	eval = false;
					break;
				}	
			}
			if(eval) 	notTakenIds.emplace_back(i); 	
		}
	}
	
	int nId = 0;
	for(int i=0; i<notTakenIds.size(); i++)
	{
		while(nId < _targetCapacity){
			if(_targets[nId] == nullptr){
				//ROS_DEBUG_NAMED("run", "create %d target", nId);
				_targets[nId] = new UKF(_ukf_params);
				_targets[nId]->box_id_ = nId; 	// set box id
				_targets[nId]->initialize(_trackBoxes[notTakenIds[i]], _trackCp[notTakenIds[i]], timeStamp);
				debug_pred_id++;
				break;
			}
			nId++;
		}
	}
}

void ImmUkf::run(double timeStamp)
{
	// record whether the measurement is taken or not, if is not taken, then add new ukf obj 
	vector<bool> measTaken(_trackCp.size(), false);

	static int removeDuplicatedTargetId = 0;
	for(int i = 0; i < _targetCapacity; i++)
	{
		if(_targets[i] == nullptr) 	continue;
		
		if(_targets[i]->state_ == UKF::trackingState_.ISSTATIC || _targets[i]->state_ == UKF::trackingState_.LOST){
			//ROS_DEBUG_NAMED("run", "target %d static or lost at lifetime(%d)", i, _targets[i]->life_time_);
			delete _targets[i];
			_targets[i] = nullptr;
			continue;
		}

		if(!_targets[i]->is_initialized_)
		{
			// association the nearest bbox
			int minId = -1;
			getNearestEuclidBBox(_targets[i]->init_cp_.back(), _trackCp, minId);
			if(minId == -1){
				ROS_DEBUG_NAMED("run", "target %d did not get nearest bbox", i);
				_targets[i]->state_ = UKF::trackingState_.LOST;
				continue;
			}
			
			measTaken[minId] = true;
			_targets[i]->initialize(_trackBoxes[minId], _trackCp[minId], timeStamp);

		}else{
		
			_targets[i]->ProcessIMMUKF(timeStamp, true);

			// prevent ukf not to explode
			double detPmerge = _targets[i]->P_merge_.determinant();
			if(detPmerge > _detP_th || _targets[i]->P_merge_(4,4) > 1000){
				_targets[i]->state_ = UKF::trackingState_.LOST;
				ROS_DEBUG_NAMED("run", "[run] targets %d P_merge(%f) explode at lifetime(%d)", i, detPmerge, _targets[i]->life_time_);
				continue;
			}

			// pre gating
			// prevent ukf not to explode
			// state covariance
			VectorXd maxDetZ;
			MatrixXd maxDetS;
			findMaxZandS(i, maxDetZ, maxDetS);
			//findBestZandS(i, maxDetZ, maxDetS);
			//maxDetS = maxDetS*4;
			double detS = maxDetS.determinant();
			if(isnan(detS) || (detS > _detS_th)) {
				_targets[i]->state_ = UKF::trackingState_.LOST;
				ROS_DEBUG_NAMED("run", "[run] targets %d detS(%f) explode at lifetime(%d)", i, detS, _targets[i]->life_time_);
				continue;
			}
		
			vector<VectorXd> measVec;
			vector<int> measIds;
			measurementValidation(i, maxDetZ, maxDetS, measVec, measTaken, measIds);
			updateTrackingState(i, measVec.size(), measVec);
			
			//ROS_DEBUG_NAMED("run", "target %d get meas(%d), (%f, %f)", i, measVec.size(), measVec[0](0), measVec[0](1));
			
			if(_targets[i]->state_ == UKF::trackingState_.LOST) 	continue;
			
			//ROS_DEBUG_NAMED("run", "target %d maxDetZ, (%f, %f)", i, maxDetZ(0), maxDetZ(1));
			associateBB(i, maxDetZ, measVec, measIds);
			if(_targets[i]->state_ >= UKF::trackingState_.DRIFT)
			{
				_targets[i]->state_ = UKF::trackingState_.LOST;
				continue;
			}
			// execute probability data associaiton filter
			// because we have 3 model, we should calculate 3 weights to combine 3 results together, that's why PDA here
			vector<double> lambdaVec;
			filterPDA(i, measVec, lambdaVec);
			// this step merge all the results together to make more accuracy inference
			_targets[i]->PostProcessIMMUKF(lambdaVec);
			
			updateBB(i, measIds);
			
			//ROS_DEBUG_NAMED("getArr", "target %d state %d", i, _targets[i]->state_);
			// predict trajectory
			_targets[i]->predictTraj(_predTime, _egoUkf.trajState_);
			 //_targets[i]->predictTraj(_predTime, ego_traj);

		}

	}
	
	 //merge is not neccessary for now
	//mergeOverSegmentation(_targets);
	
	while(removeDuplicatedTargetId < _targetCapacity)
	{
		if(_targets[removeDuplicatedTargetId] != nullptr)
		{
			removeDuplicatedTarget(removeDuplicatedTargetId);
			removeDuplicatedTargetId++;
			break;
		}
		removeDuplicatedTargetId++;
	}
	if(removeDuplicatedTargetId >= _targetCapacity) removeDuplicatedTargetId = 0;
	
	//for(int i=0; i<_targetCapacity; i++)
	//{
		//if(_targets[i] != nullptr && _targets[i]->state_ >= 7){
			//if(_targets[i]->trajState_.size() == 0)
			//{
				//try{
					//std::cout<<"haha"<<std::endl;
					//throw false;
				//}catch(bool& e){}
			//}
		//}
	//}

	// add new ukf target
	updateTargets(measTaken, timeStamp);

}

void get8PtsFromXYZAndYaw(const Vector3d& cp, const Vector3d& xyz, const double& yaw,  PointCloud<PointXYZ>& pc)
{
	Matrix<double, 3, 4> pts;
	Matrix<double, 3, 8> allpts;
	pts.setOnes();
	allpts.setZero();

	double xysign[4][2] = {{1, 1}, {1, -1}, {-1, -1}, {-1, 1}};

	for(int i=0; i<4; i++){
		pts(0, i) = xysign[i][0] * xyz(0) / 2;
		pts(1, i) = xysign[i][1] * xyz(1) / 2;
	}

	pts.row(2).array() = (-xyz(2) / 2);
	allpts.block<3, 4>(0, 0) = pts;
	pts.row(2).array() = (xyz(2) / 2);
	allpts.block<3, 4>(0, 4) = pts;

	AngleAxisd yawAngle(yaw, Vector3d::UnitZ());
	Matrix3d Rot = yawAngle.toRotationMatrix();

	allpts = Rot * allpts;

	for(int i=0; i<8; i++){
		Vector3d tmp = cp + allpts.col(i);
		PointXYZ o;
		o.x = tmp(0);
		o.y = tmp(1);
		o.z = tmp(2);
		pc.push_back(o);
	}
	
}

void ImmUkf::removeDuplicatedTarget(int id)
{
	UKF& tg1 = *(_targets[id]);
	if(tg1.state_ >= UKF::trackingState_.STABLE)
	{
		for(int i=0; i<_targetCapacity; i++)
		{
			if(_targets[i] != nullptr && id != i){
				 UKF& tg2 = *(_targets[i]);
				 VectorXd dist_vec = tg1.x_merge_.block<2, 1>(0, 0) - tg2.x_merge_.block<2, 1>(0, 0);
				 ROS_DEBUG_NAMED("removeDuplicatedTarget", "target %d to %d dist(%f)", id, i, dist_vec.norm());
				 if(dist_vec.norm() < _newTargetDistTh){
					 if(tg1.life_time_ > tg2.life_time_) tg2.state_ = UKF::trackingState_.LOST;
					 else 	tg1.state_ = UKF::trackingState_.LOST;
				 }
			}
		}
	}
}

TrackingResults ImmUkf::getArrangeResults(){
	TrackingResults tres;
	
	// get ego traj
	// for(const auto& e : _egoUkf.trajState_)
	for(const auto& e : ego_traj)
	{
		PointXYZ o;
		o.x = e[0];
		o.y = e[1];
		o.z = 0;
		tres.egoTraj.push_back(std::move(o));
	}

	for(int i=0; i<_targetCapacity; i++){
		const UKF* tg = _targets[i];
		if(tg == nullptr) 	continue;
		
		if(tg->state_ >= UKF::trackingState_.STABLE)
		{
			PointCloud<PointXYZ> pc;

			// push back 8 bbox pts
			Vector3d cp;
			cp << tg->x_merge_(0), tg->x_merge_(1), tg->init_z_cp_;
			get8PtsFromXYZAndYaw(cp, tg->Bbox_XYZ_, tg->x_merge_yaw_, pc);
			

			for(int j=0; j<tg->trajState_.size(); j++)
			{
				PointXYZ o;
				o.x = tg->trajState_[j][0];
				o.y = tg->trajState_[j][1];
				o.z = 0; 	// this one is yaw

				pc.push_back(o);
			}

			tres.bPtsAndTrajPts.emplace_back(pc);
			
			//push back xyz and yaw
			Vector4d tmp_4d;
			tmp_4d.head<3>() = tg->Bbox_XYZ_;
			tmp_4d(3) = tg->x_merge_yaw_;
			tres.bxyzAndYaw.emplace_back(tmp_4d);
			tres.vel.emplace_back(tg->x_merge_(2));
			tres.id.emplace_back(i);
			tres.color.emplace_back(tg->box_color_);
		}
		
	}

	return tres;
}
